/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/include/agvNavigate/opInclude/operation_turn_around_omni.h
 * @Description  : 定义了OperationTurnAroundOmni类，负责处理全向移动机器人在特定操作中的转向和移动逻辑。 
 *                 包含初始化、开始、结束及执行操作的相关方法，以及与ROS相关的功能实现。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 16:36:44
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/ 
#pragma once
#include "rclcpp/rclcpp.hpp"
#include "operation.h"
#include "operation_peripheral.h"
#include "trajectory.h"
#include "timestamp.h"

//////////////////////////////////////////////////////////////////////////////
//   The interface of class "OperationTurnAroundOmni".
class OperationTurnAroundOmni : public Operation
{
 public:
    explicit OperationTurnAroundOmni(const agv_msgs::msg::Action &action);

    // The destructor
    virtual ~OperationTurnAroundOmni();

    // Initialize the related data
    virtual bool Init() override;

    // Ready to start new op
    virtual bool ReadyStart() override;

    // Defines how to start the operation
    virtual bool StartAction() override;

    // Defines how to end the operation
    virtual bool EndAction() override;

    // Defines how to do the operation
    virtual bool Work() override;

    // 循环函数
    bool CycleProc();

 public:
    // for perial call back function
    void FeedbackCb(GoalHandlePeripheral::SharedPtr,
      const std::shared_ptr<const Peripheral::Feedback> feedback);

    void DoneCb(const GoalHandlePeripheral::WrappedResult & result);

    void ActiveCb(GoalHandlePeripheral::SharedPtr goal_handle);

    // 只用来测试逻辑使用
    // void DebugInit();

    // // 调试周期函数
    // void DebugCycle();

 private:
    // 数据输入来源
    enum INPUT_SOURCE {
        QR,    // 二维码
        ANALOG // 模拟量
    };
    
    enum CAMERA_TYPE {
        CAMERA_CHECK_GROUND, // 检测地面二维码
        CAMERA_CHECK_RACK    // 检测货架二维码
    };

    enum TURN_PHASE {
        PHASE_START = 0,           // 开始阶段
        PHASE_CALC_SPIN_ANGLE = 1, // 计算准备自旋的角度
        PHASE_SPIN = 2,            // 开始旋转
        PHASE_CALC_X_Y_LINE = 3,   // 计算准备平移的距离直线
        PHASE_MOVE_LINE = 4,       // 走一条直线
        PHASE_CHECK_X_Y_ANGLE = 5, // 任务完成进入等待阶段或检测是否到位
        PHASE_UPDATE_POS = 6,      // 完成等待退出
        PHASE_LOAD = 7,            // 进行取货举升阶段
        PHASE_FINISH = 8           // 完成等待退出
    };

    enum SPIN_PHASE {
        SPIN_READY_START = 0, // 矫正角度：等待自旋开始
        SPIN_START_OK = 1     // 矫正角度：自旋开始
    };

    enum LOAD_PHASE {
        LOAD_INIT = 0,
        LOAD_READY_START = 1,
        LOAD_START = 2,
        LOAD_WORK = 3,
        LOAD_READY_END = 4,
        LOAD_END = 5,
        LOAD_ERROR = 6
    };

    enum CALC_DATA_RESULT {
        RET_MAX_ADJUST_ERROR = -5,           // 超过最大矫正次数，动作执行失败
        RET_ANGLE_OVER_LIMIT_ERROR = -4,     // 二维码当前角度误差过大，不允许旋转
        RET_LOAD_STATUS_ERROR = -3,          // 货物状态异常,已经处于载货
        RET_GET_LOAD_DATA_FAILED_ERROR = -2, // 获取货物信息超时,topic无数据
        RET_GET_DATA_FAILED_ERROR = -1,      // 获取二维码数据超时

        RET_X_Y_ANGLE_TARGET = 0, // x  y  angle 是否符合0 0 0
        RET_X_Y_TARGET = 1,       // x y  达到目标
        RET_ANGLE_TARGET = 2,     // 角度达到目标
        RET_NEXT_STAGE = 3,       // 正常进入下一阶段
        RET_RESTART = 4,          // 再次进入矫正初始化循环阶段
        RET_IGNORE_ACTION = 5     // 忽略矫正动作（当小于矫正阈值）-只在开始阶段判断
    };

    struct MOVE_DATA {
        float x_target_dis;       // X方向偏差距离 正:X轴正方向 负:X轴负方向
        float y_target_dis;       // Y方向偏差距离 正:Y轴正方向，负:Y轴负方向
        float spin_target_angle;  // 自旋目标角度;正:逆时针;负:顺时针
        MOVE_DATA() : x_target_dis(0.0f), y_target_dis(0.0f), spin_target_angle(0.0f) {}
    };
    
    // 距离探测方式获取数据
    struct DisDetectMethod {
        float install_x;          // 左前右后，传感器安装x方向(取其中一个绝对值即可)
        float target_dis_1;       // 理论车体居中时，目标探测距离1-左侧
        float target_dis_2;       // 理论车体居中时，目标探测距离2-右侧
        int channel_1;            // 左前传感器模拟量通道索引 从0起
        int channel_2;            // 右后传感器模拟向通道索引

        float max_dis_detect;      // 最大探测距离m
        float ignore_y_dis_dev;    // 可以忽略的最大平移方向距离
        float ignore_angle_dis_dev;// 可以忽略角度方向的距离值 angle = dis/车长/2

        DisDetectMethod() : install_x(1.0f), target_dis_1(0.0f), 
            target_dis_2(0.0f), channel_1(0), channel_2(1),                             
            max_dis_detect(0.24f), ignore_y_dis_dev(0.02f), 
            ignore_angle_dis_dev(0.02f) {}
    };

    bool HandleLoadAcitonCycle();

    // Initialize the load related data
    bool LoadInit();

    // Ready to start new load op
    bool LoadReadyStart();

    // Defines how to start the load
    bool LoadStart();

    // Defines how to end the load
    bool LoadEnd();

    // Defines how to do the load
    bool LoadWork();

 private: 
    // ros相关函数和变量
    //  初始化ros相关变量
    bool InitRosValue();

    // 获取载货状态
    bool GetLoadStatus(bool &is_loaded);

    // 获取下读头数据
    bool GetGroundCameraData(float &pos_x, float &pos_y, float &angle);

    // 获取上读头数据
    bool GetRackCameraData(float &pos_x, float &pos_y, float &angle);

    // 获取模拟量数据；计算左前右后距离探测，计算出 x y t偏差；(特殊：x始终为0）
    bool GetAnalogPos(float &pos_x, float &pos_y, float &angle);

 private:
    // 获取计算左前右后距离探测
    bool GetAnalogDis(float &dis1, float &dis2);

    // 误差很小，可以忽略
    bool CanIgnore();

    // 距离是否超限制
    bool DisError();

    // 获取里程计的指定字段内容
    bool GetOdomData(Velocity &vel, float &theta);

    // 判断数据的时效性
    bool IsTimeOut(const rclcpp::Time &data_time, const rclcpp::Time &cur_time, const double &max_time_ms);

    // 获取实际反馈的速度
    bool GetRealVel(Velocity &vel);

    // 提取到的原始二维码数据到普通pos格式
    bool RawDataToPos(const geometry_msgs::msg::PoseStamped &camera_data_pos, Posture &output_data_pos);

    // 相对零点偏移
    bool ToRelativePos(const geometry_msgs::msg::PoseStamped &camera_data_pos, const Posture &input_relative_pos, Posture &output_data_pos);

    // 获取当前照相机的原始数据
    geometry_msgs::msg::PoseStamped GetCurCameraRawData();

 private:
    // 设置矫正阶段
    void SetPhase(int phase);

    // 初始化变量值或读取参数服务器
    void SetDefaultValue();

    // 复位运行状态变量值
    void ResetStatus();

    // 开始调整阶段
    int StartTune();

    // 设置相对参考零位(QR旋转方向)
    int SetRelativePos();

    // 自旋清除角度误差
    bool SpinMove(float angle);

    // 控制车体走一段直线轨迹
    bool StraightMove(MOVE_DATA move_data);

    // 将车体停止
    void Stop();

    // 检测照相机是否数据丢失或超时
    void CheckDataLost();

    // 清除数据丢失报警标志
    void ClearDataLostFlag();

    // 读取通用接口函数数据偏差
    bool ReadPos(float &pos_x, float &pos_y, float &angle);

    // 读取robot相对于二维码的位置偏差
    bool ReadCameraPos(int type, float &pos_x, float &pos_y, float &angle);

    // 读取里程记的当前角度
    bool GetRelAngle(float &rel_angle);

    // PI/2的整数倍数;四舍五入
    // fTheta:弧度
    int MatchToMultipleOfPi_2(float theta);

    // PI/2 的余数弧度
    // fTheta:弧度 返回值 绝对值
    float RemainderDivPi_2(float theta);

 private:
    // 是否x y angle到达目标位置
    bool IsXYAngleTarget(float x, float y, float angle);

    // 是否x y 到达目标位置
    bool IsXYTarget(float x, float y);

    // 计算自旋角度
    int CalcSpinAngle(); // bool CalcSpinAngle();

    // 计算X Y直线偏差
    int CalcXYLine();

    // 旋转到0度
    bool SpinToTarget();

    // 走直线消除x y方向偏差
    bool MoveLine();

    // 空闲阶段；是否需要再次矫正
    int CheckXYAngle();

    // 更新位姿态
    bool UpdatePosProc();

    // 清除自旋标志
    void ClearSpinFlag();

 private:
    // 读取参数表加载;到达目标位置的阈值
    float limit_x_;         // X方向（前后）矫正的到达目标的门限值 （m）
    float limit_y_;         // Y方向（左右）矫正的到达目标的门限值 (m)
    float limit_theta_;     // 角度方向矫正的到达目标的门限值（弧度）
    float max_spin_vel_;    // 最大自旋速度 rad/s
    float min_spin_vel_;    // 最小自旋速度 rad/s
    float velocity_;        // 走直线合速度 m/s
    int max_adjust_times_;  // 最大允许调整次数

    int current_adjust_times_;    // 记录当前调整的次数
    float current_line_progress_; // 走斜线时，当前短直线的进度
    int spin_phase_;        // 清除Y方向误差时，旋转步骤
    int input_source_;      // 数据输入源0:qr 1:analog
    float max_angle_error_; // 最大允许的角度误差上限制，超过则报错

    // 运行时，参数值
    int phase_;             // 当前正在进行矫正的步骤
    int data_lost_count_;   // 照相机丢失的计数
    TIME_TYPE last_time_;   // 记录上一次循环的执行的时间点
    float last_real_angle_; // 清除角度偏差时，自旋时，记录里程计上一次的相对角度值
    float micro_angle_diff_;// 清除角度偏差时，自旋时，累加微小的角度(积分角度)
    float turned_angle_;    // 清除角度偏差时，自旋时，累计已经旋转过的角度值 带有正负号
    float cycle_;           // 当前实际循环执行的时间周期
    bool data_lost_;        // 上读头相机是否报数据丢失错误
    int current_camera_type_; // 当前使用的二维码读头的类型（下读头或上读头）

    LOAD_PHASE load_phase_;        // 取货相关变量- 取货阶段
    MOVE_DATA move_data_;          // 当前循环的移动数据记录
    Posture relative_zero_pos_;    // 参考坐标系转换；旋转矢量
    DisDetectMethod dis_method_;   // 超声波左右测距方式参数
    // std::vector<ros::Subscriber> sub_vec_; // 话题订阅集
};
